#include "include/clk.h"
#include "include/flash.h"
#include "include/gpio.h"
#include "include/pie.h"
#include "include/pll.h"
#include "include/pwm.h"
#include "include/wdog.h"

#define bool int

#define EPWM1_TBPRD 19		// period register for ePWM1
#define EPWM1_CMPA 1		// sets width of clock pulse (set no higher than 18)

#define EPWMx_TBPRD 59999	// period register for all other ePWMs


/* PWM2A	Servos 5-8 (Yaw Control)
 * PWM2B	Motor Control
 * PWM3A	Servo 1 (Pitch Control)
 * PWM3B	Servo 2 (Pitch Control)
 * PWM4A	Servo 3	(Roll Control)
 * PWM5A	Servo 4 (Roll Control)
 *
 * CMP values = (desired duty cycle)*600
 *
 * Adjust these if the tolerance is too wide.
 */
#define EPWM2_CMPA_Default 4524	// 7.54% duty
#define EPWM3_CMPA_Default 4560	// 7.60% duty
#define EPWM3_CMPB_Default 4884	// 8.14% duty
#define EPWM4_CMPA_Default 4452	// 7.42% duty
#define EPWM5_CMPA_Default 4506	// 7.51% duty
#define EPWM2_CMPA_Min 2316		// 3.86% duty
#define EPWM3_CMPA_Min 2064		// 3.44% duty
#define EPWM3_CMPB_Min 2436		// 4.06% duty
#define EPWM4_CMPA_Min 2208		// 3.68% duty
#define EPWM5_CMPA_Min 2154		// 3.59% duty
#define EPWM2_CMPA_Max 6732		// 11.22% duty
#define EPWM3_CMPA_Max 7050		// 11.75% duty
#define EPWM3_CMPB_Max 7332		// 12.22% duty
#define EPWM4_CMPA_Max 6696		// 11.16% duty
#define EPWM5_CMPA_Max 6852		// 11.42% duty

#define EPWM2_CMPB_Off 1		// ~0% duty, essentially motor off
#define EPWM2_CMPB_Min 3525		// 5.875% duty
#define EPWM2_CMPB_Hover 4400	// 7.45% duty (estimate)
#define EPWM2_CMPB_Max 5718		// 9.53% duty

int saved_EPWM2_CMPA = EPWM2_CMPA_Default,
	saved_EPWM2_CMPB = EPWM2_CMPB_Off,
	saved_EPWM3_CMPA = EPWM3_CMPA_Default,
	saved_EPWM3_CMPB = EPWM3_CMPB_Default,
	saved_EPWM4_CMPA = EPWM4_CMPA_Default,
	saved_EPWM5_CMPA = EPWM5_CMPA_Default;

CLK_Handle myClk;
FLASH_Handle myFlash;
GPIO_Handle myGpio;
PIE_Handle myPie;
PWM_Handle myPwm1, myPwm2, myPwm3, myPwm4, myPwm5;

//===============Mario
int* _MEM_RX;
int _MEM_RX_COUNTER;
//==================
int main () {
	// initialize all GPIO values, PWM, etc.

    CPU_Handle myCpu;
    PLL_Handle myPll;
    WDOG_Handle myWDog;

    // Initialize all the handles needed for this application
    myClk = CLK_init((void *)CLK_BASE_ADDR, sizeof(CLK_Obj));
    myCpu = CPU_init((void *)NULL, sizeof(CPU_Obj));
    myFlash = FLASH_init((void *)FLASH_BASE_ADDR, sizeof(FLASH_Obj));
    myGpio = GPIO_init((void *)GPIO_BASE_ADDR, sizeof(GPIO_Obj));
    myPie = PIE_init((void *)PIE_BASE_ADDR, sizeof(PIE_Obj));
    myPll = PLL_init((void *)PLL_BASE_ADDR, sizeof(PLL_Obj));
    myWDog = WDOG_init((void *)WDOG_BASE_ADDR, sizeof(WDOG_Obj));

    // initialize PWM handles
    myPwm1 = PWM_init((void *)PWM_ePWM1_BASE_ADDR, sizeof(PWM_Obj));
    myPwm2 = PWM_init((void *)PWM_ePWM2_BASE_ADDR, sizeof(PWM_Obj));
    myPwm3 = PWM_init((void *)PWM_ePWM3_BASE_ADDR, sizeof(PWM_Obj));
    myPwm4 = PWM_init((void *)PWM_ePWM4_BASE_ADDR, sizeof(PWM_Obj));
    myPwm5 = PWM_init((void *)PWM_ePWM5_BASE_ADDR, sizeof(PWM_Obj));

    // Perform basic system initialization
    WDOG_disable(myWDog);
    CLK_enableAdcClock(myClk);
    (*Device_cal)();
    CLK_disableAdcClock(myClk);

    //Select the internal oscillator 1 as the clock source
    CLK_setOscSrc(myClk, CLK_OscSrc_Internal);

    // Setup the PLL for x10 /2 which will yield 50Mhz = 10Mhz * 10 / 2
    PLL_setup(myPll, PLL_Multiplier_10, PLL_DivideSelect_ClkIn_by_2);

    // Disable the PIE and all interrupts
    PIE_disable(myPie);
    PIE_disableAllInts(myPie);
    CPU_disableGlobalInts(myCpu);
    CPU_clearIntFlags(myCpu);

    // If running from flash copy RAM only functions to RAM
#ifdef _FLASH
    memcpy(&RamfuncsRunStart, &RamfuncsLoadStart, (size_t)&RamfuncsLoadSize);
#endif

    InitializePWM();

	// establish a connection with the computer
	// check if there is a route to be downloaded
	// when there is a route to be downloaded, download it
		// first and last point should be the location of the charging station
		// last element of route array should be 0 (NULL)
	// wait until the battery is fully charged
	// take off, rising until the desired height is reached
		// perform constant stability and altitude checks and adjust accordingly
	// fly from point to point on the route, checking the following in order of priority:
		// stability, from the IMU (adjust servos/control surfaces)
			/*
			 * Stability criterion will be determined through a series of tests			*
			 * and will basically be a list of limits to determine how much we can		*
			 * tilt on any axis before becoming unstable.								*
			 * If we are approaching that limit, corrections will be made to counteract *
			 * the tilt	and get us back into safe operating orientation.				*
			 * If we are stable, we are permitted to move forward in the flight checks.	*
			 * If it is determined that we are capsizing, the vehicle will terminate	*
			 * the route and attempt emergency landing procedures.						*
			 */
		// altitude, from the rangefinder (adjust motor speed)
			/*
			 * To avoid having to constantly adjust motor speed, there will be some		*
			 * "forgiveness" when determining when we should adjust altitude.  First,	*
			 * each altitude measurement is passed through a weighted average function. *
			 * This function reduces the affect of noisy terrain on our alititude		*
			 * calculations.  This weighted equation takes the form of:					*
			 * CURRENT_ALT = PREVIOUS_ALT + WEIGHT_FACTOR * (NEW_ALT - PREVIOUS_ALT)	*
			 * CURRENT_ALT:		The altitude reported to the flight logic function.		*
			 * PREVIOUS_ALT:	The previous value of CURRENT_ALT.						*
			 * WEIGHT_FACTOR:	A factor (less than 1) determining the amount new		*
			 * 					altitude measurements should have on our reported		*
			 * 					altitude.												*
			 * NEW_ALT:			The most recent altitude measurement, fresh from the	*
			 * 					ultrasonic rangefinder.									*
			 * Using a weighted function helps to ensure that sudden, drastic changes	*
			 * in altitude don't cause the vehicle to wildly change its height to		*
			 * match.																	*
			 * The further CURRENT_ALT is from the desired altitude, the more the motor *
			 * speed will change from the standard hovering RPM.  This helps us to		*
			 * adjust to smooth changes in terrain height relatively quickly, since		*
			 * the repeated measurements will have a compounding effect on our reported *
			 * altitude.																*
			 */
		// our current position and heading, from the GPS (adjust servos/control surfaces)
			/*
			 * A few different criteria are used to determine how we need to change 	*
			 * the heading of the vehicle to match the desired route.  We can use one	*
			 * or all of these critera to determine if we are on course and what		*
			 * corrective adjustments are needed to keep it that way.					*
			 * 1.)	Current position vs. Desired position: The standard criterion for	*
			 * determining the vehicle's desired heading.  So long as we know the		*
			 * vehicle's directional orientation (most likely from the IMU's magneto-	*
			 * meter) we can make adjustments that bring us closer to the next point on *
			 * the route.																*
			 * 2.)	Distance from ideal route: The criterion for determining whether	*
			 * the security footage we're collecting is relevant to the route we're		*
			 * supposed to be following.  This distance will be determined through		*
			 * trigonometry, and will influence the heading determined by step 1.  The	*
			 * distance measured will fall into one of four discrete categories:		*
			 * "Optimal":	The vehicle is closely following the ideal route.  No		*
			 * changes are made to the heading determined by step 1.					*
			 * "Acceptable":	The vehicle is drifting off-course.  An adjustment will *
			 * be made to the step 1 heading in order to bring the vehicle closer to	*
			 * the ideal route.															*
			 * "Unacceptable":	The vehicle is no longer recording footage relevant to	*
			 * the route defined by the user.  The heading from step 1 will be replaced *
			 * with one taking the vehicle directly towards the ideal route.			*
			 * "Lost":	The vehicle is very very far from the ideal route and returning *
			 * to it is not an option.  A flag will be thrown, the flight loop will be	*
			 * broken, and emergency landing procedures will be executed.				*
			 * 3.)	Projected heading vs. actual heading: This helps to account for		*
			 * environmental factors (specifically, air currents) that may be pushing	*
			 * the vehicle.  By measuring the angle between the vectors describing the	*
			 * heading we attempted to achieve with the previous adjustment and the		*
			 * heading we actually achieved from it, we can determine what in which		*
			 * direction and to what magnitude our course was altered.  An adjustment	*
			 * will be made if the angle is above a predetermined value.				*
			 */
		// broadcast current position to computer terminal
	// when all of the points have been visited, we attempt to land
		/*
		 * There are two landing conditions:
		 * 1.) Emergency: Attempt to set the vehicle on the ground as gently as		*
		 * possible.  Once the vehicle has landed and has stopped moving, it		*
		 * will repeatedly broadcast its current GPS coordinates to the computer	*
		 * terminal.																*
		 * 2.) Docking Attempt: The vehicle will use its IR camera to look for a 	*
		 * ring of infrared LEDs on the bottom of the docking station.  As it		*
		 * descends, it will attempt to keep the ring of LEDs in the center of the	*
		 * IR sensor.  This will ensure the vehicle stays on course as it descends. *
		 * If the ring of LEDs is lost, the vehicle will switch to emergency		*
		 * landing mode.															*
		 *
		 *
		 * Note: I'm really banking on the idea that we'll be able to get a matrix
		 * representation of what the IR camera sees.  If we can represent the view as a
		 * 2-dimensional array, I can easily keep the vehicle on course.  We need
		 * to look into this soon.
		 */
}

void InitializePWM() {
	/* PWM2A	Servos 5-8 (Yaw Control)
	 * PWM2B	Motor Control
	 * PWM3A	Servo 1 (Pitch Control)
	 * PWM3B	Servo 2 (Pitch Control)
	 * PWM4A	Servo 3	(Roll Control)
	 * PWM5A	Servo 4 (Roll Control)
	 */

	// set the necessary pins to PWM mode
	GPIO_setPullUp(myGpio, GPIO_Number_0, GPIO_PullUp_Disable);
	GPIO_setMode(myGpio, GPIO_Number_0, GPIO_0_Mode_EPWM1A);

	GPIO_setPullUp(myGPio, GPIO_Number_2, GPIO_PullUp_Disable);
	GPIO_setMode(myGpio, GPIO_Number_2, GPIO_2_Mode_EPWM2A);

	GPIO_setPullUp(myGPio, GPIO_Number_3, GPIO_PullUp_Disable);
	GPIO_setMode(myGpio, GPIO_Number_3, GPIO_3_Mode_EPWM2B);

	GPIO_setPullUp(myGpio, GPIO_Number_4, GPIO_PullUp_Disable);
	GPIO_setMode(myGpio, GPIO_Number_4, GPIO_4_Mode_EPWM3A);

	GPIO_setPullUp(myGPio, GPIO_Number_5, GPIO_PullUp_Disable);
	GPIO_setMode(myGpio, GPIO_Number_5, GPIO_5_Mode_EPWM3B);

	GPIO_setPullUp(myGpio, GPIO_Number_6, GPIO_PullUp_Disable);
	GPIO_setMode(myGpio, GPIO_Number_6, GPIO_6_Mode_EPWM4A);

	GPIO_setPullUp(myGPio, GPIO_Number_8, GPIO_PullUp_Disable);
	GPIO_setMode(myGpio, GPIO_Number_8, GPIO_8_Mode_EPWM5A);

	// set GPIO 32 to accept the 3MHz clock signal output from ePWM1A
	GPIO_setPullUp(myGpio, GPIO_Number_32, GPIO_PullUp_Disable);
	GPIO_setMode(myGpio, GPIO_Number_32, GPIO_32_Mode_EPWMSYNCI);

	// enable the clocks for all of these
	CLK_enablePwmClock(myClk, PWM_Number_1);
	CLK_enablePwmClock(myClk, PWM_Number_2);
	CLK_enablePwmClock(myClk, PWM_Number_3);
	CLK_enablePWMClock(myClk, PWM_Number_4);
	CLK_enablePWMClock(myClk, PWM_Number_5);

	// set up the period and phase of each PWM and initialize the counters
	PWM_setPeriod(myPwm1, EPWM1_TBPRD);
	PWM_setPhase(myPwm1, 0x0000);		// we do not want any phase on our pulse
	PWM_setCount(myPwm1, 0x0000);		// initializes the counter to zero

	PWM_setPeriod(myPwm2, EPWMx_TBPRD);
	PWM_setPhase(myPwm2, 0x0000);
	PWM_setCount(myPwm2, 0x0000);

	PWM_setPeriod(myPwm3, EPWMx_TBPRD);
	PWM_setPhase(myPwm3, 0x0000);
	PWM_setCount(myPwm3, 0x0000);

	PWM_setPeriod(myPwm4, EPWMx_TBPRD);
	PWM_setPhase(myPwm4, 0x0000);
	PWM_setCount(myPwm4, 0x0000);

	PWM_setPeriod(myPwm5, EPWMx_TBPRD);
	PWM_setPhase(myPwm5, 0x0000);
	PWM_setCount(myPwm5, 0x0000);

	// we don't need any of the high-res functions of PWM1.
	PWM_disableHrPeriod(myPwm1);
	PWM_disableHrPhaseSync(myPwm1);

	// set the compares for each of the ePWM modules
	PWM_setCmpA(myPwm1, EPWM1_CMPA);

	PWM_setCmpA(myPwm2, EPWM2_CMPA_Default);
	PWM_setCmpB(myPwm2, EPWM2_CMPB_Off);

	PWM_setCmpA(myPwm3, EPWM3_CMPA_Default);
	PWM_setCmpB(myPwm3, EPWM3_CMPB_Default);

	PWM_setCmpA(myPwm4, EPWM4_CMPA_Default);

	PWM_setCmpA(myPwm5, EPWM5_CMPA_Default);

	// counter counts from 0 to EPWMx_TBPRD and then returns to 0
	PWM_setCounterMode(myPwm1, PWM_CounterMode_Up);

	PWM_setHighSpeedClkDiv(myPwm1, PWM_HspClkDiv_by_1);
	PWM_setClkDiv(myPwm1, PWM_ClkDiv_by_1);
	PWM_setClkDiv(myPwm2, PWM_ClkDiv_by_1);
	PWM_setClkDiv(myPwm3, PWM_ClkDiv_by_1);
	PWM_setClkDiv(myPwm4, PWM_ClkDiv_by_1);
	PWM_setClkDiv(myPwm5, PWM_ClkDiv_by_1);
	// we do not divide the clock signal in the prescaler

	// setup shadowing (uses registers to make changes to CMPA synchronous)
	PWM_setShadowMode_CmpA(myPwm1, PWM_ShadowMode_Shadow);
	PWM_setLoadMode_CmpA(myPwm1, PWM_LoadMode_Zero);

	PWM_setShadowMode_CmpA(myPwm2, PWM_ShadowMode_Shadow);
	PWM_setLoadMode_CmpA(myPwm2, PWM_LoadMode_Zero);
	PWM_setShadowMode_CmpB(myPwm2, PWM_ShadowMode_Shadow);
	PWM_setLoadMode_CmpB(myPwm2, PWM_LoadMode_Zero);

	PWM_setShadowMode_CmpA(myPwm3, PWM_ShadowMode_Shadow);
	PWM_setLoadMode_CmpA(myPwm3, PWM_LoadMode_Zero);
	PWM_setShadowMode_CmpB(myPwm3, PWM_ShadowMode_Shadow);
	PWM_setLoadMode_CmpB(myPwm3, PWM_LoadMode_Zero);

	PWM_setShadowMode_CmpA(myPwm4, PWM_ShadowMode_Shadow);
	PWM_setLoadMode_CmpA(myPwm4, PWM_LoadMode_Zero);

	PWM_setShadowMode_CmpA(myPwm5, PWM_ShadowMode_Shadow);
	PWM_setLoadMode_CmpA(myPwm5, PWM_LoadMode_Zero);

	// set actions (set at the beginning of each period, reset when the counter equals CMPA)
	PWM_setActionQual_Zero_PwmA(myPwm1, PWM_ActionQual_Set);
	PWM_setActionQual_CntUp_CmpA_PwmA(myPwm1, PWM_ActionQual_Clear);

	PWM_setActionQual_Zero_PwmA(myPwm2, PWM_ActionQual_Set);
	PWM_setActionQual_CntUp_CmpA_PwmA(myPwm2, PWM_ActionQual_Clear);
	PWM_setActionQual_Zero_PwmB(myPwm2, PWM_ActionQual_Set);
	PWM_setActionQual_CntUp_CmpB_PwmB(myPwm2, PWM_ActionQual_Clear);

	PWM_setActionQual_Zero_PwmA(myPwm3, PWM_ActionQual_Set);
	PWM_setActionQual_CntUp_CmpA_PwmA(myPwm3, PWM_ActionQual_Clear);
	PWM_setActionQual_Zero_PwmB(myPwm3, PWM_ActionQual_Set);
	PWM_setActionQual_CntUp_CmpB_PwmB(myPwm3, PWM_ActionQual_Clear);

	PWM_setActionQual_Zero_PwmA(myPwm4, PWM_ActionQual_Set);
	PWM_setActionQual_CntUp_CmpA_PwmA(myPwm4, PWM_ActionQual_Clear);

	PWM_setActionQual_Zero_PwmA(myPwm5, PWM_ActionQual_Set);
	PWM_setActionQual_CntUp_CmpA_PwmA(myPwm5, PWM_ActionQual_Clear);


	// None of the PWMs should cause interrupts
	PWM_disableInt(myPwm1);
	PWM_disableInt(myPwm2);
	PWM_disableInt(myPwm3);
	PWM_disableInt(myPwm4);
	PWM_disableInt(myPwm5);

	// allow the 3MHz clock to flow through each PWM module to the next by setting the sync mode
	// so that EPWMxSYNCO = EPWMxSYNCI
	PWM_setSyncMode(myPwm1, PWM_SyncMode_EPWMxSYNC);
	PWM_setSyncMode(myPwm2, PWM_SyncMode_EPWMxSYNC);
	PWM_setSyncMode(myPwm3, PWM_SyncMode_EPWMxSYNC);
	PWM_setSyncMode(myPwm4, PWM_SyncMode_EPWMxSYNC);

	// make PWM1 ignore the 3MHz signal
	PWM_disableCounterLoad(myPwm1);

	// make PWM2-PWM5 use the 3MHz signal
	PWM_enableCounterLoad(myPwm2);
	PWM_enableCounterLoad(myPwm3);
	PWM_enableCounterLoad(myPwm4);
	PWM_enableCounterLoad(myPwm5);

	return;
}

void changePWM2ADutyCycle(int new_EPWM2_CMPA) {
	if(new_EPWM2_CMPA > EPWM2_CMPA_Max)
		PWM_setCmpA(myPwm2, saved_EPWM2_CMPA = EPWM2_CMPA_Max);
	else if(new_EPWM2_CMPA < EPWM2_CMPA_Min)
		PWM_setCMPA(myPwm2, saved_EPWM2_CMPA = EPWM2_CMPA_Min);
	else
		PWM_setCMPA(myPwm2, saved_EPWM2_CMPA = new_EPWM2_CMPA);
}

void changePWM2BDutyCycle(int new_EPWM2_CMPB) {
	if(new_EPWM2_CMPB > EPWM2_CMPB_Max)
		PWM_setCmpB(myPwm2, saved_EPWM2_CMPB = EPWM2_CMP2_Max);
	else if(new_EPWM2_CMPB < EPWM2_CMPB_Min)
		PWM_setCmpB(myPwm2, saved_EPWM2_CMPB = EPWM2_CMP2_Min);
	else
		PWM_setCmpB(myPwm2, saved_EPWM2_CMPB = new_EPWM2_CMPB);
}

void changePWM3ADutyCycle(int new_EPWM3_CMPA) {
	if(new_EPWM3_CMPA > EPWM3_CMPA_Max)
		PWM_setCmpA(myPwm3, saved_EPWM3_CMPA = EPWM3_CMPA_Max);
	else if(new_EPWM3_CMPA < EPWM3_CMPA_Min)
		PWM_setCmpA(myPwm3, saved_EPWM3_CMPA = EPWM3_CMPA_Min);
	else
		PWM_setCmpA(myPwm3, saved_EPWM3_CMPA = new_EPWM3_CMPA);
}

void changePWM3BDutyCycle(int new_EPWM3_CMPB) {
	if(new_EPWM3_CMPB > EPWM3_CMPB_Max)
		PWM_setCmpB(myPwm3, saved_EPWM3_CMPB = EPWM3_CMPB_Max);
	else if(new_EPWM3_CMPB < EPWM3_CMPB_Min)
		PWM_setCmpB(myPwm3, saved_EPWM3_CMPB = EPWM3_CMPB_Min);
	else
		PWM_setCmpB(myPwm3, saved_EPWM3_CMPB = new_EPWM3_CMPB);
}

void changePWM4ADutyCycle(int new_EPWM4_CMPA) {
	if(new_EPWM4_CMPA > EPWM4_CMPA_Max)
		PWM_setCmpA(myPwm4, saved_EPWM4_CMPA = EPWM4_CMPA_Max);
	else if(new_EPWM4_CMPA < EPWM4_CMPA_Min)
		PWM_setCmpA(myPwm4, saved_EPWM4_CMPA = EPWM4_CMPA_Min);
	else
		PWM_setCmpA(myPwm4, saved_EPWM4_CMPA = new_EPWM4_CMPA);
}

void changePWM5ADutyCycle(int new_EPWM5_CMPA) {
	if(new_EPWM5_CMPA > EPWM5_CMPA_Max)
		PWM_setCmpA(myPwm5, saved_EPWM5_CMPA = EPWM5_CMPA_Max);
	else if(new_EPWM5_CMPA < EPWM5_CMPA_Min)
		PWM_setCmpA(myPwm5, saved_EPWM5_CMPA = EPWM5_CMPA_Min);
	else
		PWM_setCmpA(myPwm5, saved_EPWM5_CMPA = new_EPWM5_CMPA);
}

void resetPWM2A() {
	changePWM2ADutyCycle(EPWM2_CMPA_Default);
}

void resetPWM3A() {
	changePWM3ADutyCycle(EPWM3_CMPA_Default);
}

void resetPWM3B() {
	changePWM3BDutyCycle(EPWM3_CMPB_Default);
}

void resetPWM4A() {
	changePWM4ADutyCycle(EPWM4_CMPA_Default);
}

void resetPWM5A() {
	changePWM5ADutyCycle(EPWM5_CMPA_Default);
}

void resetAllServos() {
	resetPWM2A();
	resetPWM3A();
	resetPWM3B();
	resetPWM4A();
	resetPWM5A();
}


//============Mario

void scia_xmit(int a)
{
    while (SciaRegs.TXRDY != 1){}//. != 0) {}
    SciaRegs.SCITXBUF=a;
}

void scia_msg(char * msg)
{
    int i;
    i = 0;
    while(msg[i] != '\0')
    {
        scia_xmit(msg[i]);
        i++;
    }
}

void rx_mem_init(){
	_MEM_RX = malloc(sizeof(int));
	int _MEM_RX_COUNTER = 0;
}

//===============
